#include "test.h"
#include "plan.h"
#include "tim.h"

void setUp(void)
{
}

void tearDown(void)
{
}

float tic(void)
{
    HAL_GPIO_WritePin(TEST_IO_GPIO_Port, TEST_IO_Pin, GPIO_PIN_SET);
    return (float)__HAL_TIM_GET_COUNTER(&htim5) * TIMER_COEF;
}

float toc(void)
{
    HAL_GPIO_WritePin(TEST_IO_GPIO_Port, TEST_IO_Pin, GPIO_PIN_RESET);
    return (float)__HAL_TIM_GET_COUNTER(&htim5) * TIMER_COEF;
}

void testJointPlanner(void)
{
    JointPlanner_TypeDef *planner = joint_planner_new();
    int ret = joint_planner_init(0.0f, 1.0f, 0.0f, 0.0001f, planner);
    float t = 0.0f, p;
    float start_time, end_time;

    while (1)
    {
        start_time = tic();
        t += 0.1f;
        p = joint_plan_move(planner, t);
        end_time = toc();
        LOG_INFO("position = %f, cost %f ms", p, (end_time - start_time));
        TEST_ASSERT_EQUAL_FLOAT(0.0f, 0.0f);
        HAL_Delay(100);
    }
}

void run_test(void)
{
    testJointPlanner();
}
